/*
 * Servo output control
 *
 * by Zik Saleeba 2008-02-22
 */

#include "ffox.h"
#include "AT91SAM7X256.h"
#include "lib_AT91SAM7X256.h"
#include "servo.h"


/* module variables */
static unsigned int ServoPos[SERVO_NUM_TIMESLOTS];
static unsigned int ServoTicks[SERVO_NUM_TIMESLOTS];
int CurrentPort;
static int ServoPin[SERVO_NUM_TIMESLOTS] = 
{ 
    SERVO1_OUTPUT, 
    SERVO2_OUTPUT, 
    SERVO3_OUTPUT, 
    SERVO4_OUTPUT, 
    SERVO5_OUTPUT, 
    0
};    


/* prototypes */
void Servo_Isr_Handler_IRQ(void) __attribute__ ((interrupt("IRQ")));
void Servo_Isr_Handler_FIQ(void) __attribute__ ((interrupt("FIQ")));


/*
 * NAME:        Servo_Init
 * ACTION:      Initialise the servo subsystem. This sets up timer 2
 *              to do the timing.
 */
 
void Servo_Init( void )
{
    int Count;
   
	/* set up the initial positions for each channel */
	for (Count = 0; Count < SERVO_NUM_TIMESLOTS; Count++)
	{
	    ServoPos[Count] = SERVO_MID_US;
	    ServoTicks[Count] = TIMER2_TIMESLOT_TICKS * (SERVO_MID_US + SERVO_MIN_US) / SERVO_TIMESLOT_US;
	}
	
	CurrentPort = 0;
	
    /* disable the timer for programming */
    AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKDIS;
    AT91C_BASE_TC2->TC_IDR = 0xFFFFFFFF;
    AT91C_BASE_TC2->TC_SR;

    /* Disable the FIQ interrupt */
    AT91F_AIC_DisableIt(AT91C_BASE_AIC, AT91C_ID_FIQ);

    /* configure the interrupts on the interrupt controller */
    AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_TC2, 7, AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &Servo_Isr_Handler_IRQ);
    AT91C_BASE_AIC->AIC_SVR[ AT91C_ID_FIQ ] = (unsigned int)Servo_Isr_Handler_FIQ;

    /* set up TIMER2 to 50Hz/SERVO_NUM_TIMESLOTS with interrupts on "RA" (the 
     * pulse duration) and "RC" (the timeslot duration) */
    AT91C_BASE_TC2->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | 
                             AT91C_TC_WAVESEL_UP_AUTO |
                             AT91C_TC_CPCTRG |
                             AT91C_TC_WAVE;
    
    AT91C_BASE_TC2->TC_CV = 0;
    AT91C_BASE_TC2->TC_RC = (AT91_REG)TIMER2_TIMESLOT_TICKS;           /* the period */
    AT91C_BASE_TC2->TC_RA = (AT91_REG)TIMER2_TIMESLOT_TICKS*SERVO_MID_US/SERVO_TIMESLOT_US;       /* TIOA2 goes high at 1.5ms in */

    /* enable timer interrupts */
    AT91C_BASE_TC2->TC_IDR = 0xFF; 
    AT91C_BASE_TC2->TC_IER = AT91C_TC_CPAS | AT91C_TC_CPCS;

    /* make this a fast forcing interrupt */
    AT91C_BASE_AIC->AIC_FFER = 0x1 << AT91C_ID_TC2;
    AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_TC2);
    
    /* start the timer */
    AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
}


/*
 * NAME:        Servo_Set
 * ACTION:      Set the position of a servo
 * PARAMETERS:  int Port - which servo to move (0-6)
 *              int Position - what position to move to (in ms)
 */
 
void Servo_Set(int Port, int Position)
{
    if (Port >= 0 && Port < NUM_SERVO_OUTPUTS &&
        Position >= SERVO_SENSIBLE_POSITION_MIN && Position <= SERVO_SENSIBLE_POSITION_MAX)
    {
        ServoPos[Port] = Position;
        ServoTicks[Port] = TIMER2_TIMESLOT_TICKS * (Position + SERVO_MIN_US) / SERVO_TIMESLOT_US;
    }
}


/*
 * NAME:        Servo_Get
 * ACTION:      Get the current position of a servo
 * PARAMETERS:  int Port - which servo to get (1-7)
 * RETURNS:     int - the current servo position (in ms)
 */
 
int Servo_Get(int Port)
{
    if (Port >= 0 && Port < NUM_SERVO_OUTPUTS)
        return ServoPos[Port];
    else
        return 0;
}


/*
 * NAME:        Servo_Isr_Handler
 * ACTION:      Interrupt service routine for servo control.
 *              We get interrupted in two cases:
 *                  1) when the timer reaches RC it's automatically
 *                     reset. At this point we set the appropriate
 *                     servo pin.
 *                  2) when the timer reaches RA we clear the
 *                     servo pin and load up RA with the next value.
 *                     This will be loaded on the next RC reset.
 */
 
void Servo_Isr_Handler_IRQ(void)
{
    register unsigned int TimerStatus;
    
    /* what event got us here? */
    TimerStatus = AT91C_BASE_TC2->TC_SR;
    if (TimerStatus & AT91C_TC_CPCS)
    {
        /* the end of a timeslot has been reached - set the new pin */
    	CurrentPort++;
    	if (CurrentPort >= SERVO_NUM_TIMESLOTS)
    	    CurrentPort = 0;
    	    
        if (CurrentPort < NUM_SERVO_OUTPUTS)
    	    AT91F_PIO_SetOutput(AT91C_BASE_PIOA, ServoPin[CurrentPort]);
    	
    	/* load up the next port's pulse width in advance */
    	AT91C_BASE_TC2->TC_RA = ServoTicks[CurrentPort];
    }
    else if (TimerStatus & AT91C_TC_CPAS)
    {
        /* the pulse width has been reached - clear the pin */
        if (CurrentPort < NUM_SERVO_OUTPUTS)
    	    AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, ServoPin[CurrentPort]);
    }

    /* clear the interrupt in the AIC to complete interrupt handling */ 
    AT91C_BASE_AIC->AIC_EOICR = 0;
}

void Servo_Isr_Handler_FIQ(void)
{
    register unsigned int TimerStatus;
    
    /* what event got us here? */
    TimerStatus = AT91C_BASE_TC2->TC_SR;
    if (TimerStatus & AT91C_TC_CPCS)
    {
        /* the end of a timeslot has been reached - set the new pin */
    	CurrentPort++;
    	if (CurrentPort >= SERVO_NUM_TIMESLOTS)
    	    CurrentPort = 0;
    	    
        if (CurrentPort < NUM_SERVO_OUTPUTS)
    	    AT91F_PIO_SetOutput(AT91C_BASE_PIOA, ServoPin[CurrentPort]);
    	
    	/* load up the next port's pulse width in advance */
    	AT91C_BASE_TC2->TC_RA = ServoTicks[CurrentPort];

        /* clear the interrupt in the AIC */ 
        AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_TC2);
    }
    else if (TimerStatus & AT91C_TC_CPAS)
    {
        /* the pulse width has been reached - clear the pin */
        if (CurrentPort < NUM_SERVO_OUTPUTS)
    	    AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, ServoPin[CurrentPort]);

        /* clear the interrupt in the AIC */ 
        AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_TC2);
    }
}
